﻿using System;
using System.Collections;
using System.Collections.Generic;


public class EquationsSolving  {
    


}

/// <summary>
/// 线形方程
/// </summary>
public class LinearEquationSolving
{
    public const int precision = 100;

    LinearEquation[] m_LinearEquations = new LinearEquation[0];
    public LinearEquation[] linearEquations => m_LinearEquations;

    List<Unknown> m_AllUnknowns;
    public List<Unknown> allUnknowns => m_AllUnknowns;

    public LinearEquationSolving(LinearEquation[] _linearEquations)
    {
        m_LinearEquations = _linearEquations;

        m_AllUnknowns = GetAllUnknowns(_linearEquations);

        if (m_LinearEquations.Length < m_AllUnknowns.Count)
        {
#if LOG
            GameDebug.LogError("方程数比未知数个数少，无法解");
#endif
        }
        else
        {
#if LOG
            GameDebug.Log($"方程组个数：{m_LinearEquations.Length}");
#endif
        }
        usedLE = new List<LinearEquation>();
        for (int i = 0; i < m_LinearEquations.Length; i++)
            CalculateUnknown2(m_LinearEquations);

#if LOG
        for (int i = 0; i < m_AllUnknowns.Count; i++)
        {
            if (!m_AllUnknowns[i].knownValue)
            {
                GameDebug.LogError($"未算出未知数：{m_AllUnknowns[i].name}");
            }
            else
            {
                GameDebug.Log($"求出未知数：{m_AllUnknowns[i]}：{m_AllUnknowns[i].value}");
            }
        }
#endif
    }

    static List<Unknown> GetAllUnknowns(LinearEquation[] _linearEquations)
    {
        var unknowns = new List<Unknown>();

        for (int i = 0; i < _linearEquations.Length; i++)
        {
            if (_linearEquations[i].unknownExpressions != null)
            {
                for (int j = 0; j < _linearEquations[i].unknownExpressions.Length; j++)
                {
                    var unknown = _linearEquations[i].unknownExpressions[j].x;

                    if (!unknowns.Contains(unknown))
                        unknowns.Add(unknown);
                }
            }
        }
        return unknowns;
    }

    static string GetString(LinearEquation[] _linearEquations)
    {
        string str = "{\n";
        for (int i = 0; i < _linearEquations.Length; i++)
        {
            str += _linearEquations[i].ToString()+"\n";
        }
        str += "}\n";
        return str;
    }

    static List<LinearEquation> usedLE = new List<LinearEquation>();
    /// <summary>
    /// 求出一个未知数
    /// </summary>
    /// <param name="_linearEquations"></param>
    /// <param name="cur"></param>
    /// <param name="curUnknown"></param>
    static void CalculateUnknown2(LinearEquation[] _linearEquations)
    {
        if (_linearEquations.Length <= 0)
            return;

#if LOG
        GameDebug.Log($"求方程组：{GetString(_linearEquations)}   _linearEquations.Length：{_linearEquations.Length}  ");
#endif

        Unknown curUnknown = null;

        LinearEquation cur = null;
        FixedFloat curFactor = 0;
        bool find = false;
        for (int j = 0; j < _linearEquations.Length; j++)
        {
            foreach (var ue in _linearEquations[j].unknownExpressions)
            {
                if (!ue.x.knownValue && ue.factor != 0)
                {
                    curUnknown = ue.x;
                    cur = _linearEquations[j];
                    curFactor = ue.factor;

                    find = true;
                    break;
                }
            }
            if (find)
            {
                break;
            }
        }

#if LOG
        GameDebug.Log($"第一个包含未知数{curUnknown}的方程：{cur}");
#endif
        if (curUnknown == null)
            return;

        bool canGetValue = TryGetValue(cur, ref curUnknown);
        if (canGetValue)
        {
            usedLE.Add(cur);

#if LOG
            GameDebug.LogShowy($"能直接求出值：{curUnknown}");
#endif
            //return;// continue;
        }
        else
        {
#if LOG
            GameDebug.Log($"消元 消掉：{curUnknown}   _linearEquations.Length：{_linearEquations.Length}");
#endif
            List<LinearEquation> newList = new List<LinearEquation>();
            LinearEquation next = null;
            FixedFloat nextFactor = 0;

            for (int i = 0; i < _linearEquations.Length; i++)
            {
                if (cur == _linearEquations[i])
                    continue;
                
                bool findUnknown = false;
                foreach (var ue in _linearEquations[i].unknownExpressions)
                {
                    if (ue.x == curUnknown)
                    {
                        if (ue.factor == 0)
                            continue;

                        findUnknown = true;
                        
                        next = _linearEquations[i];
                        nextFactor = ue.factor;

                        if (nextFactor != 0)
                        {
                            //消元
                            FixedFloat f = -nextFactor / curFactor;
                            var curMulti = cur.SetTargetFactor(curUnknown, -1 * nextFactor);// cur.Multi(f);
                            LinearEquation newLE = curMulti.Add(next);// curMulti.Add(next);

#if LOG
                            GameDebug.Log($"{curMulti} 加 {next} 结合生成方程  add ：{newLE}");
#endif
                            newList.Add(newLE);
                        }
                        else
                        {
                            //newList.Add(_linearEquations[i]);
                        }
                        break;
                    }
                }
                if (!findUnknown)
                {
                    //GameDebug.Log($"!findUnknown{curUnknown}   add：{_linearEquations[i]}");
                    newList.Add(_linearEquations[i]);
                }
            }

            CalculateUnknown2(newList.ToArray());
        }
    }
    
    public static bool TryGetValue(LinearEquation linearEquation,ref Unknown unknown)
    {
        var firstEx = linearEquation.GetExprssion(unknown);

        FixedFloat value;
        
        ///直接求出
        bool canGetValue = firstEx.GetValue(out value);
        
        if (canGetValue)
        {
            //value = FixedMath.Rough(value, LinearEquationSolving.precision);

            unknown.knownValue = true;
            unknown.value = value;

#if LOG
            GameDebug.Log($"求出未知数{unknown}：{value}");
#endif
            return true;
        }
        return false;
    }

    /// <summary>
    /// 代入
    /// </summary>
    /// <param name="toChangeExpression"></param>
    /// <param name="unknown"></param>
    /// <param name="unknownExpression"></param>
    /// <returns></returns>
    public static LinearEquation Substitution(LinearEquation toChangeExpression,Unknown unknown,Expression unknownExpression)
    {
        List<UnknownExpression> res = new List<UnknownExpression>();
        List<UnknownExpression> allToAdd = new List<UnknownExpression>();


        FixedFloat newC = toChangeExpression.C;
        string allToAddStr = "";

        for (int i = 0; i < toChangeExpression.unknownExpressions.Length; i++)
        {
            var ue = toChangeExpression.unknownExpressions[i];

            if(ue.x == unknown)
            {
                for (int j = 0; j < unknownExpression.expressions.Length;j++)
                {
                    var eToAdd = unknownExpression.expressions[j];
                    FixedFloat newFactor = eToAdd.factor * ue.factor;
                    eToAdd.factor = newFactor;
                    allToAddStr += eToAdd.ToString() + ", ";

                    allToAdd.Add(eToAdd);
                }

                newC += unknownExpression.C * ue.factor;
#if LOG
                GameDebug.Log($"toChangeExpression.C:{toChangeExpression.C}  temp.factor{ue.factor} * {unknownExpression.C}  newC:{newC}");
#endif
            }
            else
            {
                allToAddStr += ue.ToString()+", ";
                allToAdd.Add(ue);
            }
        }

#if LOG
        GameDebug.Log($"{toChangeExpression} 里  unknown:{unknown} 替换成：{unknownExpression}  allToAdd:{allToAddStr}");
#endif

        foreach (var toAdd in allToAdd)
        {
            bool find = false;
            for (int j = 0; j < res.Count; j++)
            {
                if (res[j].x == toAdd.x)
                {
                    find = true;

                    var temp = res[j];
                    FixedFloat factor = temp.factor + toAdd.factor;
                    temp.factor = factor;
                   // GameDebug.Log($"已存在 系数相+:{temp.factor}+{toAdd.factor}   factor:{factor}");
                    res[j] = temp;

#if LOG
                    GameDebug.Log($"已存在 系数相+{res[j].factor}");
#endif
                    //break;
                }
            }

            if (!find)
            {
                res.Add(toAdd);
            }
        }

       // newC = toChangeExpression.C + unknownExpression.C;
        LinearEquation linearEquation = new LinearEquation(res.ToArray(), newC);

#if LOG
        GameDebug.Log($"{unknown}={unknownExpression} 代入{toChangeExpression} 结果是：{linearEquation}");
#endif
        return linearEquation;
    }

    
}

/// <summary>
/// ax + by + cz + C = 0; Expression = 0
/// </summary>
public class LinearEquation
{
    UnknownExpression[] m_UnknownExpressions;
    public UnknownExpression[] unknownExpressions => m_UnknownExpressions;

    FixedFloat m_C;
    public FixedFloat C=>m_C;

    public LinearEquation(UnknownExpression[] _unknownExpression, FixedFloat C)
    {
        m_UnknownExpressions = _unknownExpression;
        this.m_C = C;
    }

    public bool HasUnknown(Unknown unknown)
    {
        for (int i = 0; i < this.m_UnknownExpressions.Length; i++)
        {
            UnknownExpression cur = m_UnknownExpressions[i];

            if (cur.x == unknown)
            {
                return true;
            }
        }
        return false;
    }
    public bool GetUnknownExprssion(Unknown unknown,out UnknownExpression ex)
    {
        for (int i = 0; i < this.m_UnknownExpressions.Length; i++)
        {
            UnknownExpression cur = m_UnknownExpressions[i];

            if (cur.x == unknown)
            {
                ex = cur;
                return true;
            }
        }
        ex = new UnknownExpression();
        return false;
    }

    public Expression GetExprssion(Unknown unknown)
    {
       // bool find = false;
        System.Nullable<UnknownExpression> find = null; 
        List<UnknownExpression> temp = new List<UnknownExpression>();

        for (int i = 0; i < this.m_UnknownExpressions.Length; i++)
        {
            UnknownExpression cur = m_UnknownExpressions[i];

            if (cur.x == unknown)
            {
                find = cur; 
            }
            else
            {
                FixedFloat newFactor = -1 * cur.factor;
                temp.Add(new UnknownExpression(newFactor, cur.x));
            }
        }
        string tempStr = "";
        if (!find.HasValue)
        {
            return null;
        }
        else
        {
            for(int i=0;i< temp.Count;i++)
            {
                var e = temp[i];
                e.factor = temp[i].factor / find.Value.factor;
                //GameDebug.Log($"{temp[i].factor} / {find.Value.factor}   e.factor:{ e.factor}" );

                temp[i] = e;

                tempStr += $"{temp[i]}, ";
            }
        }
        //GameDebug.Log($"tempStr:{tempStr} ");


     Expression expression = new Expression(temp.ToArray(), -1 * m_C / find.Value.factor);

        FixedFloat value;
        bool knownValue = expression.GetValue(out value);
        //GameDebug.Log($"{this} 求{unknown} 的表达式是：{expression}  是否已知值：{knownValue}  值：{value}");

        return expression;
    }

    public LinearEquation Multi(FixedFloat multi)
    {
        return Multi(this, multi);
    }
    public  LinearEquation SetTargetFactor(Unknown unknown, FixedFloat factor)
    {
        return SetTargetFactor(this, unknown, factor);
    }
    public static LinearEquation SetTargetFactor(LinearEquation linearEquation,Unknown unknown ,FixedFloat factor)
    {
        LinearEquation res = null;
#if LOG
       // GameDebug.Log($"方程:{linearEquation}乘除使未知数:{unknown} 系数为{factor}");
#endif

        var ue = System.Array.Find(linearEquation.unknownExpressions, e => e.x == unknown);
        for (int i = 0; i < linearEquation.unknownExpressions.Length; i++)
        {
            var cur = linearEquation.unknownExpressions[i];
            if(cur.x == unknown)
            {
                //消元
                FixedFloat f = factor / cur.factor;

                var newLE = linearEquation.Multi(f);

                for (int j=0;j< newLE.unknownExpressions.Length;j++)
                {
                    var ne = newLE.unknownExpressions[j];
                    if (ne.x == unknown)
                    {
#if LOG
                        //GameDebug.Log($"乘以{f}  后是{ne.factor}  直接设为：{factor}");
#endif
                        ne.factor = factor;
                        newLE.unknownExpressions[j] = ne;
                        break;
                    }
                }
                res = newLE;
                break ;

            }
        }
        return res;
    }

    public static LinearEquation Multi(LinearEquation linearEquation , FixedFloat multi)
    {
        int length = linearEquation.unknownExpressions.Length;
        UnknownExpression[] unknownExpressions = new UnknownExpression[length];
        for (int i = 0; i < length; i++)
        {
            var ue = linearEquation.unknownExpressions[i];
            unknownExpressions[i] = new UnknownExpression(ue.factor,ue.x);
        }

        LinearEquation res = new LinearEquation(unknownExpressions, linearEquation.C);

        for (int i = 0; i < res.unknownExpressions.Length; i++)
        {
            UnknownExpression cur = res.unknownExpressions[i];

            cur.factor *= multi;

            res.unknownExpressions[i] = cur;
        }
        res.m_C *= multi;

        return res;
    }

    public LinearEquation Add(LinearEquation other)
    {
        List<UnknownExpression> expressions = new List<UnknownExpression>();
        //  LinearEquation res = new LinearEquation();
        expressions.AddRange(m_UnknownExpressions);

#if LOG
        //GameDebug.Log($"{this}与 {other}相加");
#endif
        for (int i = 0; i < other.unknownExpressions.Length; i++)
        {
            UnknownExpression cur = other.unknownExpressions[i];

            bool find = false;
            for (int j = 0; j < expressions.Count; j++)
            {
                if (expressions[j].x == cur.x)
                {
                    var temp = expressions[j];
                    FixedFloat f = (temp.factor + cur.factor);

#if LOG
                    //GameDebug.Log($"{cur.x}: {temp.factor} + {cur.factor} = {f} ");
#endif
                    temp.factor = f;
#if LOG
                    //GameDebug.Log($"{f}   temp.factor :{  temp.factor }");
#endif
                    expressions[j] = temp;

                    find = true;
                    break;
                }
            }
            if(!find)
            {
                expressions.Add(cur);
            }
        }
        for (int j = 0; j < expressions.Count; j++)
        {
            FixedFloat fAbs = expressions[j].factor>0? expressions[j].factor:-expressions[j].factor;
            if (fAbs < new FixedFloat(0.0002))// 0.0001f )
            {
                expressions.RemoveAt(j);
            }
        }
        FixedFloat newC = this.C + other.C;

        return new LinearEquation(expressions.ToArray(), newC);
    }
    
    public override string ToString()
    {
        string res = "";

        for (int i = 0; i < this.m_UnknownExpressions.Length; i++)
        {
            if (i != 0)
                res += "+";
            res += m_UnknownExpressions[i].ToString();
        }
        res += "+" + m_C;
        res += "= 0";

        return res;
    }
}

public class Expression
{
    UnknownExpression[] m_Expressions;
    public UnknownExpression[] expressions => m_Expressions;


    //*
    FixedFloat m_A;
    public FixedFloat A => m_A;

    //除
    FixedFloat m_B;
    public FixedFloat B => m_B;

    //+
    FixedFloat m_C;
    public FixedFloat C => m_C;
    
    public Expression(UnknownExpression[] _expressions, /*FixedFloat A, FixedFloat B,*/ FixedFloat C)
    {
        if (_expressions == null)
        {
            throw new System.ArgumentNullException("_expressions == null");
        }
        

        this.m_Expressions = _expressions;
        //this.m_A = A;
        //this.m_B = B;
        this.m_C = C;
        
    }

    public bool GetValue(out FixedFloat value)
    {
        value = 0;
        bool canGetValue = true;

        FixedFloat res = 0;
        foreach (var e in expressions)
        {
            if (e.factor == 0)
                continue;

            if (!e.x.knownValue)
                canGetValue = false;
            else
                res += e.value;
        }
        res += m_C;
        value = res;
        return canGetValue;
    }

    public bool HasUnknown(Unknown unknown)
    {
        for (int i = 0; i < this.m_Expressions.Length; i++)
        {
            UnknownExpression cur = m_Expressions[i];

            if (cur.x == unknown)
            {
                return true;
            }
        }
        return false;
    }

    public override string ToString()
    {
        string res = "";

        for (int i = 0; i < this.m_Expressions.Length; i++)
        {
            if (i != 0)
                res += "+";
            res += m_Expressions[i].ToString();
        }

        res += "+" + m_C;

        return res;
    }
}

/// <summary>
/// 表达式
/// </summary>
public struct UnknownExpression
{
    FixedFloat m_Factor;
    public FixedFloat factor
    {
        get { return m_Factor; }
        set { m_Factor = value; }
    }

    Unknown m_X;
    public Unknown x => m_X;

    public FixedFloat value => m_X.knownValue ? m_Factor * m_X.value : 0;

    public UnknownExpression(FixedFloat _factor, Unknown x)
    {
        m_X = x;
        m_Factor = _factor;
    }

    public Unknown GetUnknownValue(FixedFloat  targetValue)
    {
        if (m_Factor == 0)
        {
            m_X.knownValue = false;
            return m_X;
        }
        m_X.value = targetValue / m_Factor;
        m_X.knownValue = true;
        return m_X;
    }

    public  bool Equal(UnknownExpression obj)
    {
        return this.m_X == obj.x 
            && this.m_Factor == obj.factor;
    }

    public override string ToString()
    {
        string res = m_Factor + m_X.name;
        return res;
    }
}

/// <summary>
/// 未知数
/// </summary>
public class Unknown
{
    public string name;
    public bool knownValue;
    public FixedFloat value;

    public Unknown(string name)
    {
        this.name = name;
    }

    public override string ToString()
    {
        return name;
    }
    //public FixedFloat Mul(FixedFloat b)
    //{

    //}
}